<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.17"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>OpenLoong Dynamics Control: foot_placement.cpp Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/x-mathjax-config">
  MathJax.Hub.Config({
    extensions: ["tex2jax.js"],
    jax: ["input/TeX","output/HTML-CSS"],
});
</script>
<script type="text/javascript" async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/MathJax.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">OpenLoong Dynamics Control
   &#160;<span id="projectnumber">v0.1.0</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.17 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
var searchBox = new SearchBox("searchBox", "search",false,'Search');
/* @license-end */
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(function() {
  initMenu('',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
/* @license-end */</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_692f640cc74363071c5c0e14282656e9.html">algorithm</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">foot_placement.cpp</div>  </div>
</div><!--header-->
<div class="contents">
<div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="comment">/*</span></div>
<div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;<span class="comment">This is part of OpenLoong Dynamics Control, an open project for the control of biped robot,</span></div>
<div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="comment">Copyright (C) 2024 Humanoid Robot (Shanghai) Co., Ltd, under Apache 2.0.</span></div>
<div class="line"><a name="l00004"></a><span class="lineno">    4</span>&#160;<span class="comment">Feel free to use in any purpose, and cite OpenLoong-Dynamics-Control in any style, to contribute to the advancement of the community.</span></div>
<div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;<span class="comment"> &lt;https://gitee.com/panda_23/openloong-dyn-control.git&gt;</span></div>
<div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="comment"> &lt;web@openloong.org.cn&gt;</span></div>
<div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="comment">*/</span></div>
<div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#include &quot;foot_placement.h&quot;</span></div>
<div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="preprocessor">#include &quot;bezier_1D.h&quot;</span></div>
<div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160; </div>
<div class="line"><a name="l00011"></a><span class="lineno"><a class="line" href="classFootPlacement.html#abefe39cf532cd6ee56094fd96b9aead1">   11</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="classFootPlacement.html#abefe39cf532cd6ee56094fd96b9aead1">FootPlacement::dataBusRead</a>(<a class="code" href="structDataBus.html">DataBus</a> &amp;robotState) {</div>
<div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;    posStart_W=robotState.<a class="code" href="structDataBus.html#a10764ddec95c7b01b4d00e75c19e4b48">swingStartPos_W</a>;</div>
<div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;    desV_W=robotState.<a class="code" href="structDataBus.html#a05de3821b1aa986adc8a98ec8469d55a">js_vel_des</a>;</div>
<div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;    <a class="code" href="classFootPlacement.html#a65426884931e20e457a72ebd2885ca91">desWz_W</a>=robotState.<a class="code" href="structDataBus.html#a176bf04fd501acfa68ee3083532df705">js_omega_des</a>(2);</div>
<div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;    <a class="code" href="classFootPlacement.html#a2c65ca56953f4f104ff55200656cffee">curV_W</a>=robotState.<a class="code" href="structDataBus.html#a0113f7940144a8e35089fd9818e8d70b">base_rot</a>*robotState.<a class="code" href="structDataBus.html#a1e20ca2abda0e8342aa07eaf4b7adaa4">dq</a>.block&lt;3,1&gt;(0,0);</div>
<div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;    <a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>=robotState.<a class="code" href="structDataBus.html#a356f98de11378aa6fbdf67c6076a6868">phi</a>;</div>
<div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;    hipPos_W=robotState.<a class="code" href="structDataBus.html#a2dd269f857bfd6dd3928e524b0b41b05">posHip_W</a>;</div>
<div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;    <a class="code" href="classFootPlacement.html#aa56a97026449da92c7bb7e65374a26bc">base_pos</a>=robotState.<a class="code" href="structDataBus.html#a7bb7e1397ba963e17e1f840cc7abd610">base_pos</a>;</div>
<div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;    <a class="code" href="classFootPlacement.html#a3aa4e1578da7623da69007b491e8765b">tSwing</a>= robotState.<a class="code" href="structDataBus.html#a0872af337cf744df13666d1ca70ddfab">tSwing</a>;</div>
<div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;    theta0=robotState.<a class="code" href="structDataBus.html#ac2e4fb0dfd07ba322070e3bd6b204e33">theta0</a>;</div>
<div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;    yawCur=robotState.<a class="code" href="structDataBus.html#a9b45a5287b92f51cc27899eee18f9ff2">rpy</a>[2];</div>
<div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;    omegaZ_W=robotState.<a class="code" href="structDataBus.html#aafbebba614c6bc9e1cebd6a432a44ad2">base_omega_W</a>(2);</div>
<div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;    hip_width=robotState.<a class="code" href="structDataBus.html#a97c72f31ccfbc780ca768b6df47d5b8b">width_hips</a>;</div>
<div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;    <a class="code" href="classFootPlacement.html#a8bc8c4f7d3ffc58d244cfc3935e31c13">legState</a>=robotState.<a class="code" href="structDataBus.html#af3616a7ffbfec988f239485c4b550a2f">legState</a>;</div>
<div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;}</div>
<div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160; </div>
<div class="line"><a name="l00027"></a><span class="lineno"><a class="line" href="classFootPlacement.html#a833b5bb6ef5578bfed4d8d50c9786fa7">   27</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="classFootPlacement.html#a833b5bb6ef5578bfed4d8d50c9786fa7">FootPlacement::dataBusWrite</a>(<a class="code" href="structDataBus.html">DataBus</a> &amp;robotState) {</div>
<div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;    robotState.<a class="code" href="structDataBus.html#a286e3619258ff05a05c1c7858caa03fa">swing_fe_rpy_des_W</a>&lt;&lt;0,0,robotState.<a class="code" href="structDataBus.html#af03f33eb3cb6bcf43ff21cee18b2a2e6">base_rpy_des</a>(2); <span class="comment">// WARNING! ThetaZ!</span></div>
<div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;    robotState.<a class="code" href="structDataBus.html#aa0e8b071bd5d432229b0f31ef839c3f3">swing_fe_pos_des_W</a>&lt;&lt;pDesCur[0],pDesCur[1],pDesCur[2];</div>
<div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;}</div>
<div class="line"><a name="l00031"></a><span class="lineno"><a class="line" href="classFootPlacement.html#a44ed054253f62f43df8b23f60694d600">   31</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="classFootPlacement.html#a44ed054253f62f43df8b23f60694d600">FootPlacement::getSwingPos</a>() {</div>
<div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;    Eigen::Matrix3d KP;</div>
<div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;    KP.setZero();</div>
<div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;    KP(0,0)=<a class="code" href="classFootPlacement.html#ae58f1f2ea1c2d690d10f4c7b7b0c67d0">kp_vx</a>;KP(1,1)=<a class="code" href="classFootPlacement.html#a9e1f6a5e58ce202d031a17be6652ce20">kp_vy</a>;KP(2,2)=0;</div>
<div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160; </div>
<div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;    <span class="comment">// for linear velocity</span></div>
<div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;    posDes_W=hipPos_W+KP*(desV_W-<a class="code" href="classFootPlacement.html#a2c65ca56953f4f104ff55200656cffee">curV_W</a>)*(-1)+0.5*<a class="code" href="classFootPlacement.html#a3aa4e1578da7623da69007b491e8765b">tSwing</a>*<a class="code" href="classFootPlacement.html#a2c65ca56953f4f104ff55200656cffee">curV_W</a>+</div>
<div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;            <a class="code" href="classFootPlacement.html#a2c65ca56953f4f104ff55200656cffee">curV_W</a>*(1-<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>)*<a class="code" href="classFootPlacement.html#a3aa4e1578da7623da69007b491e8765b">tSwing</a>;</div>
<div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160; </div>
<div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;    <span class="comment">// for angular veloctity</span></div>
<div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;    <span class="keywordtype">double</span> thetaF;</div>
<div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;    thetaF=yawCur+theta0+omegaZ_W*(1-<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>)*<a class="code" href="classFootPlacement.html#a3aa4e1578da7623da69007b491e8765b">tSwing</a>+0.5*omegaZ_W*<a class="code" href="classFootPlacement.html#a3aa4e1578da7623da69007b491e8765b">tSwing</a>+<a class="code" href="classFootPlacement.html#ad375e1c3c0dfa0c73e13372b092a0738">kp_wz</a>*(omegaZ_W-<a class="code" href="classFootPlacement.html#a65426884931e20e457a72ebd2885ca91">desWz_W</a>);</div>
<div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;    posDes_W(0)+=0.5*hip_width* (cos(thetaF)-cos(yawCur+theta0));</div>
<div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;    posDes_W(1)+=0.5*hip_width* (sin(thetaF)-sin(yawCur+theta0));</div>
<div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160; </div>
<div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;    posDes_W(0)=posDes_W(0) - 0.02;</div>
<div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;<span class="comment">//    posDes_W(2)=STPos_W(2)-0.04;</span></div>
<div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;    posDes_W(2)=<a class="code" href="classFootPlacement.html#aa56a97026449da92c7bb7e65374a26bc">base_pos</a>(2)-<a class="code" href="classFootPlacement.html#ab5b95a95c66387cf179dc54179d956a0">legLength</a>-0.03;</div>
<div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160; </div>
<div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;    <span class="keywordtype">double</span> yOff=0.01; <span class="comment">// positive for moving the leg inside</span></div>
<div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classFootPlacement.html#a8bc8c4f7d3ffc58d244cfc3935e31c13">legState</a>==<a class="code" href="structDataBus.html#a0dcb8a4552bb6b37259a61d601cd7d7ea08979273410aa36b9c631368513397ec">DataBus::LSt</a>)</div>
<div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;        posDes_W(1)+=yOff;</div>
<div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;    <span class="keywordflow">else</span> <span class="keywordflow">if</span> (<a class="code" href="classFootPlacement.html#a8bc8c4f7d3ffc58d244cfc3935e31c13">legState</a>==DataBus::RSt)</div>
<div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;        posDes_W(1)-=yOff;</div>
<div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160; </div>
<div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;    <span class="comment">// cycloid trajectories</span></div>
<div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a> &lt; 1.0){</div>
<div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;        pDesCur[0]=posStart_W(0)+(posDes_W(0)-posStart_W(0))/(2*3.1415)*(2*3.1415*<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>-sin(2*3.1415*<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>));</div>
<div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;        pDesCur[1]=posStart_W(1)+(posDes_W(1)-posStart_W(1))/(2*3.1415)*(2*3.1415*<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>-sin(2*3.1415*<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>));</div>
<div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;    }</div>
<div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;<span class="comment">//    pDesCur[2]=posStart_W(2)+stepHeight*0.5*(1-cos(2*3.1415*phi))+(posDes_W(2)-posStart_W(2))/(2*3.1415)*(2*3.1415*phi-sin(2*3.1415*phi));</span></div>
<div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160; </div>
<div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;    pDesCur[2]=posStart_W(2)+<a class="code" href="classFootPlacement.html#ae2211312c822d1d49d61553fbf692feb">Trajectory</a>(0.2, <a class="code" href="classFootPlacement.html#ab42c8e066a9f7d1997ca7780af64c932">stepHeight</a>, posDes_W(2)-posStart_W(2));</div>
<div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;}</div>
<div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160; </div>
<div class="line"><a name="l00066"></a><span class="lineno"><a class="line" href="classFootPlacement.html#ae2211312c822d1d49d61553fbf692feb">   66</a></span>&#160;<span class="keywordtype">double</span>    <a class="code" href="classFootPlacement.html#ae2211312c822d1d49d61553fbf692feb">FootPlacement::Trajectory</a>(<span class="keywordtype">double</span> phase, <span class="keywordtype">double</span> des1, <span class="keywordtype">double</span> des2){</div>
<div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;    <a class="code" href="classBezier__1D.html">Bezier_1D</a> Bswpid;</div>
<div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;    <span class="keywordtype">double</span>  para0 = 5, para1 = 3;</div>
<div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; para0; i ++)</div>
<div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;        Bswpid.<a class="code" href="classBezier__1D.html#a36e64027940c12bc42d9a0bf25cf6cbf">P</a>.push_back(0.0);</div>
<div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; para1; i ++)</div>
<div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;        Bswpid.<a class="code" href="classBezier__1D.html#a36e64027940c12bc42d9a0bf25cf6cbf">P</a>.push_back(1.0);</div>
<div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160; </div>
<div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;    <span class="keywordtype">double</span> Bsw1, Bsw2;</div>
<div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;    Bsw1 = Bswpid.<a class="code" href="classBezier__1D.html#a2f0a069e38bf26c79f1b545f62f6da04">getOut</a>(<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>/phase);</div>
<div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;    Bsw2 = Bswpid.<a class="code" href="classBezier__1D.html#a2f0a069e38bf26c79f1b545f62f6da04">getOut</a>((1.4 - <a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a>)/(1.4 - phase));</div>
<div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160; </div>
<div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;    <span class="keywordtype">double</span> output;</div>
<div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;    <span class="keywordflow">if</span> (<a class="code" href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">phi</a> &lt; phase)</div>
<div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;        output = des1*Bsw1;</div>
<div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;    <span class="keywordflow">else</span>{</div>
<div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;        <span class="keywordflow">if</span> (Bsw2 &gt; 0)</div>
<div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;            output = des1 * Bsw2 + des2 * (1.0 - Bsw2);</div>
<div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;        <span class="keywordflow">else</span></div>
<div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;            output = des2;</div>
<div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;    }</div>
<div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160; </div>
<div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;    <span class="keywordflow">return</span> output;</div>
<div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;}</div>
<div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160; </div>
</div><!-- fragment --></div><!-- contents -->
<div class="ttc" id="astructDataBus_html_a286e3619258ff05a05c1c7858caa03fa"><div class="ttname"><a href="structDataBus.html#a286e3619258ff05a05c1c7858caa03fa">DataBus::swing_fe_rpy_des_W</a></div><div class="ttdeci">Eigen::Vector3d swing_fe_rpy_des_W</div><div class="ttdoc">desired swing foot end angle position in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00136">data_bus.h:136</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_ab5b95a95c66387cf179dc54179d956a0"><div class="ttname"><a href="classFootPlacement.html#ab5b95a95c66387cf179dc54179d956a0">FootPlacement::legLength</a></div><div class="ttdeci">double legLength</div><div class="ttdoc">length of the leg</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00023">foot_placement.h:23</a></div></div>
<div class="ttc" id="astructDataBus_html_a1e20ca2abda0e8342aa07eaf4b7adaa4"><div class="ttname"><a href="structDataBus.html#a1e20ca2abda0e8342aa07eaf4b7adaa4">DataBus::dq</a></div><div class="ttdeci">Eigen::VectorXd dq</div><div class="ttdoc">velocity variable of the robot</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00051">data_bus.h:51</a></div></div>
<div class="ttc" id="astructDataBus_html_aa0e8b071bd5d432229b0f31ef839c3f3"><div class="ttname"><a href="structDataBus.html#aa0e8b071bd5d432229b0f31ef839c3f3">DataBus::swing_fe_pos_des_W</a></div><div class="ttdeci">Eigen::Vector3d swing_fe_pos_des_W</div><div class="ttdoc">desired swing foot end position in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00135">data_bus.h:135</a></div></div>
<div class="ttc" id="astructDataBus_html_a10764ddec95c7b01b4d00e75c19e4b48"><div class="ttname"><a href="structDataBus.html#a10764ddec95c7b01b4d00e75c19e4b48">DataBus::swingStartPos_W</a></div><div class="ttdeci">Eigen::Vector3d swingStartPos_W</div><div class="ttdoc">swing foot end start position in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00150">data_bus.h:150</a></div></div>
<div class="ttc" id="astructDataBus_html_a0872af337cf744df13666d1ca70ddfab"><div class="ttname"><a href="structDataBus.html#a0872af337cf744df13666d1ca70ddfab">DataBus::tSwing</a></div><div class="ttdeci">double tSwing</div><div class="ttdoc">swing period</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00158">data_bus.h:158</a></div></div>
<div class="ttc" id="astructDataBus_html_a7bb7e1397ba963e17e1f840cc7abd610"><div class="ttname"><a href="structDataBus.html#a7bb7e1397ba963e17e1f840cc7abd610">DataBus::base_pos</a></div><div class="ttdeci">Eigen::Vector3d base_pos</div><div class="ttdoc">base position in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00066">data_bus.h:66</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_aa56a97026449da92c7bb7e65374a26bc"><div class="ttname"><a href="classFootPlacement.html#aa56a97026449da92c7bb7e65374a26bc">FootPlacement::base_pos</a></div><div class="ttdeci">Eigen::Vector3d base_pos</div><div class="ttdoc">current position of robot base in world coordinate</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00030">foot_placement.h:30</a></div></div>
<div class="ttc" id="astructDataBus_html_aafbebba614c6bc9e1cebd6a432a44ad2"><div class="ttname"><a href="structDataBus.html#aafbebba614c6bc9e1cebd6a432a44ad2">DataBus::base_omega_W</a></div><div class="ttdeci">Eigen::Vector3d base_omega_W</div><div class="ttdoc">body angle velocity expressed in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00102">data_bus.h:102</a></div></div>
<div class="ttc" id="astructDataBus_html_a356f98de11378aa6fbdf67c6076a6868"><div class="ttname"><a href="structDataBus.html#a356f98de11378aa6fbdf67c6076a6868">DataBus::phi</a></div><div class="ttdeci">double phi</div><div class="ttdoc">phase varialbe for trajectory generation, must between 0 and 1, used in footPlacement,...</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00159">data_bus.h:159</a></div></div>
<div class="ttc" id="astructDataBus_html_a2dd269f857bfd6dd3928e524b0b41b05"><div class="ttname"><a href="structDataBus.html#a2dd269f857bfd6dd3928e524b0b41b05">DataBus::posHip_W</a></div><div class="ttdeci">Eigen::Vector3d posHip_W</div><div class="ttdoc">hip position in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00153">data_bus.h:153</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a2c65ca56953f4f104ff55200656cffee"><div class="ttname"><a href="classFootPlacement.html#a2c65ca56953f4f104ff55200656cffee">FootPlacement::curV_W</a></div><div class="ttdeci">Eigen::Vector3d curV_W</div><div class="ttdoc">desired and current body velocity in world coordinate</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00028">foot_placement.h:28</a></div></div>
<div class="ttc" id="astructDataBus_html_af03f33eb3cb6bcf43ff21cee18b2a2e6"><div class="ttname"><a href="structDataBus.html#af03f33eb3cb6bcf43ff21cee18b2a2e6">DataBus::base_rpy_des</a></div><div class="ttdeci">Eigen::Vector3d base_rpy_des</div><div class="ttdoc">desired body euler angle in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00129">data_bus.h:129</a></div></div>
<div class="ttc" id="astructDataBus_html_a176bf04fd501acfa68ee3083532df705"><div class="ttname"><a href="structDataBus.html#a176bf04fd501acfa68ee3083532df705">DataBus::js_omega_des</a></div><div class="ttdeci">Eigen::Vector3d js_omega_des</div><div class="ttdoc">angle velocity command from joystick controller in body frame, for now, only omega along z-axis is co...</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00111">data_bus.h:111</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_ad375e1c3c0dfa0c73e13372b092a0738"><div class="ttname"><a href="classFootPlacement.html#ad375e1c3c0dfa0c73e13372b092a0738">FootPlacement::kp_wz</a></div><div class="ttdeci">double kp_wz</div><div class="ttdoc">Catersian swing foot position control gain in z-axis.</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00022">foot_placement.h:22</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_ae58f1f2ea1c2d690d10f4c7b7b0c67d0"><div class="ttname"><a href="classFootPlacement.html#ae58f1f2ea1c2d690d10f4c7b7b0c67d0">FootPlacement::kp_vx</a></div><div class="ttdeci">double kp_vx</div><div class="ttdoc">Catersian swing foot position control gain in x-axis.</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00020">foot_placement.h:20</a></div></div>
<div class="ttc" id="astructDataBus_html_a0113f7940144a8e35089fd9818e8d70b"><div class="ttname"><a href="structDataBus.html#a0113f7940144a8e35089fd9818e8d70b">DataBus::base_rot</a></div><div class="ttdeci">Eigen::Matrix3d base_rot</div><div class="ttdoc">base rotation matrix in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00069">data_bus.h:69</a></div></div>
<div class="ttc" id="aclassBezier__1D_html_a36e64027940c12bc42d9a0bf25cf6cbf"><div class="ttname"><a href="classBezier__1D.html#a36e64027940c12bc42d9a0bf25cf6cbf">Bezier_1D::P</a></div><div class="ttdeci">std::vector&lt; double &gt; P</div><div class="ttdoc">a sequence of number used to generate a one-dimesionanl Bezier curve</div><div class="ttdef"><b>Definition:</b> <a href="bezier__1D_8h_source.html#l00019">bezier_1D.h:19</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a9e1f6a5e58ce202d031a17be6652ce20"><div class="ttname"><a href="classFootPlacement.html#a9e1f6a5e58ce202d031a17be6652ce20">FootPlacement::kp_vy</a></div><div class="ttdeci">double kp_vy</div><div class="ttdoc">Catersian swing foot position control gain in y-axis.</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00021">foot_placement.h:21</a></div></div>
<div class="ttc" id="astructDataBus_html_a9b45a5287b92f51cc27899eee18f9ff2"><div class="ttname"><a href="structDataBus.html#a9b45a5287b92f51cc27899eee18f9ff2">DataBus::rpy</a></div><div class="ttdeci">double rpy[3]</div><div class="ttdoc">roll, pitch, yaw angle of the robot body in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00030">data_bus.h:30</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a833b5bb6ef5578bfed4d8d50c9786fa7"><div class="ttname"><a href="classFootPlacement.html#a833b5bb6ef5578bfed4d8d50c9786fa7">FootPlacement::dataBusWrite</a></div><div class="ttdeci">void dataBusWrite(DataBus &amp;robotState)</div><div class="ttdoc">Write swing foot information to databus.</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8cpp_source.html#l00027">foot_placement.cpp:27</a></div></div>
<div class="ttc" id="astructDataBus_html_ac2e4fb0dfd07ba322070e3bd6b204e33"><div class="ttname"><a href="structDataBus.html#ac2e4fb0dfd07ba322070e3bd6b204e33">DataBus::theta0</a></div><div class="ttdeci">double theta0</div><div class="ttdoc">offset yaw angle of the swing foot(+pi/2 for left and -pi/2 for right), w.r.t body frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00156">data_bus.h:156</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_ae2211312c822d1d49d61553fbf692feb"><div class="ttname"><a href="classFootPlacement.html#ae2211312c822d1d49d61553fbf692feb">FootPlacement::Trajectory</a></div><div class="ttdeci">double Trajectory(double phase, double des1, double des2)</div><div class="ttdoc">Generate the desired swing leg z-position using Besier curve.</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8cpp_source.html#l00066">foot_placement.cpp:66</a></div></div>
<div class="ttc" id="astructDataBus_html_a97c72f31ccfbc780ca768b6df47d5b8b"><div class="ttname"><a href="structDataBus.html#a97c72f31ccfbc780ca768b6df47d5b8b">DataBus::width_hips</a></div><div class="ttdeci">double width_hips</div><div class="ttdoc">distance between the left and right hip</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00157">data_bus.h:157</a></div></div>
<div class="ttc" id="aclassBezier__1D_html_a2f0a069e38bf26c79f1b545f62f6da04"><div class="ttname"><a href="classBezier__1D.html#a2f0a069e38bf26c79f1b545f62f6da04">Bezier_1D::getOut</a></div><div class="ttdeci">double getOut(double s)</div><div class="ttdoc">Get the value in s.</div><div class="ttdef"><b>Definition:</b> <a href="bezier__1D_8cpp_source.html#l00032">bezier_1D.cpp:32</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a3aa4e1578da7623da69007b491e8765b"><div class="ttname"><a href="classFootPlacement.html#a3aa4e1578da7623da69007b491e8765b">FootPlacement::tSwing</a></div><div class="ttdeci">double tSwing</div><div class="ttdoc">swing time</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00026">foot_placement.h:26</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a65426884931e20e457a72ebd2885ca91"><div class="ttname"><a href="classFootPlacement.html#a65426884931e20e457a72ebd2885ca91">FootPlacement::desWz_W</a></div><div class="ttdeci">double desWz_W</div><div class="ttdoc">desired angular velocity projected on z-axis</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00029">foot_placement.h:29</a></div></div>
<div class="ttc" id="aclassBezier__1D_html"><div class="ttname"><a href="classBezier__1D.html">Bezier_1D</a></div><div class="ttdoc">This class can be used to constructed a 1-dimensional Bezier curve.</div><div class="ttdef"><b>Definition:</b> <a href="bezier__1D_8h_source.html#l00016">bezier_1D.h:16</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a44ed054253f62f43df8b23f60694d600"><div class="ttname"><a href="classFootPlacement.html#a44ed054253f62f43df8b23f60694d600">FootPlacement::getSwingPos</a></div><div class="ttdeci">void getSwingPos()</div><div class="ttdoc">Calculate the desired position of the swing foot.</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8cpp_source.html#l00031">foot_placement.cpp:31</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a8bc8c4f7d3ffc58d244cfc3935e31c13"><div class="ttname"><a href="classFootPlacement.html#a8bc8c4f7d3ffc58d244cfc3935e31c13">FootPlacement::legState</a></div><div class="ttdeci">DataBus::LegState legState</div><div class="ttdoc">record the leg state, used to make swing motion inward</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00059">foot_placement.h:59</a></div></div>
<div class="ttc" id="astructDataBus_html_a05de3821b1aa986adc8a98ec8469d55a"><div class="ttname"><a href="structDataBus.html#a05de3821b1aa986adc8a98ec8469d55a">DataBus::js_vel_des</a></div><div class="ttdeci">Eigen::Vector3d js_vel_des</div><div class="ttdoc">velocity command from joystick controller in world frame</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00112">data_bus.h:112</a></div></div>
<div class="ttc" id="astructDataBus_html_a0dcb8a4552bb6b37259a61d601cd7d7ea08979273410aa36b9c631368513397ec"><div class="ttname"><a href="structDataBus.html#a0dcb8a4552bb6b37259a61d601cd7d7ea08979273410aa36b9c631368513397ec">DataBus::LSt</a></div><div class="ttdeci">@ LSt</div><div class="ttdoc">left leg stance</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00166">data_bus.h:166</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_ab42c8e066a9f7d1997ca7780af64c932"><div class="ttname"><a href="classFootPlacement.html#ab42c8e066a9f7d1997ca7780af64c932">FootPlacement::stepHeight</a></div><div class="ttdeci">double stepHeight</div><div class="ttdoc">the maximal height of the foot when walking a step</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00024">foot_placement.h:24</a></div></div>
<div class="ttc" id="astructDataBus_html_af3616a7ffbfec988f239485c4b550a2f"><div class="ttname"><a href="structDataBus.html#af3616a7ffbfec988f239485c4b550a2f">DataBus::legState</a></div><div class="ttdeci">LegState legState</div><div class="ttdoc">current leg stance state</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00172">data_bus.h:172</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_a7efde20d6d1358c936a6d97b98993413"><div class="ttname"><a href="classFootPlacement.html#a7efde20d6d1358c936a6d97b98993413">FootPlacement::phi</a></div><div class="ttdeci">double phi</div><div class="ttdoc">phase varialbe for trajectory generation, must between 0 and 1</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8h_source.html#l00025">foot_placement.h:25</a></div></div>
<div class="ttc" id="astructDataBus_html"><div class="ttname"><a href="structDataBus.html">DataBus</a></div><div class="ttdoc">The software model for data transporting.</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00022">data_bus.h:22</a></div></div>
<div class="ttc" id="aclassFootPlacement_html_abefe39cf532cd6ee56094fd96b9aead1"><div class="ttname"><a href="classFootPlacement.html#abefe39cf532cd6ee56094fd96b9aead1">FootPlacement::dataBusRead</a></div><div class="ttdeci">void dataBusRead(DataBus &amp;robotState)</div><div class="ttdoc">Read data from databus.</div><div class="ttdef"><b>Definition:</b> <a href="foot__placement_8cpp_source.html#l00011">foot_placement.cpp:11</a></div></div>
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.17
</small></address>
</body>
</html>
